#!/usr/bin/env python
# -*- coding: UTF-8 -*-

import math,os,time,json
import rospy
from geometry_msgs.msg import Point, Pose
from v3_msgs.msg import(
    MapBase,PolygonData,
    TaskBase,TaskControlMode,TaskSpeed,
    PathGenerateType,PathGenerate,PathGenerateAction,
    TaskType,TaskSingle,TimerTask,TaskGroup,TaskState,
    OnlineTaskCtrl,OnlineTask,
    TaskEcho,TaskRecord
)
from v3_msgs.srv import (
    TaskGenerateManager, TaskGenerateManagerRequest, 
    TaskSingleManager, TaskSingleManagerRequest,
    TimerTaskManager, TimerTaskManagerRequest,
    TaskGroupManager, TaskGroupManagerRequest,
    TaskReportManager, TaskReportManagerRequest
)
from cln_msgs.srv import(
    GenerateTeachPath, GenerateTeachPathRequest
)
from tool import Log_Parser,get_extract_ip
log = Log_Parser()


class TaskParser:
    def __init__(self):
        self.__node_name = "TaskParser"
        self.__response_timeout = 3.0 #等待响应超时，单位：s
        self.__taskecho = TaskEcho()

        self.__online_tasks_deque = OnlineTask() #在线任务队列
        self.__task_generate_client = rospy.ServiceProxy("/task_generate_server",TaskGenerateManager)
        self.__task_single_client = rospy.ServiceProxy("/task_manager_server",TaskSingleManager)
        self.__timertask_manager_client = rospy.ServiceProxy("/timertask_manager_server",TimerTaskManager)
        self.__grouptask_manager_client = rospy.ServiceProxy("/taskgroup_manager_server",TaskGroupManager)
        # self.__teaching_path_client = rospy.ServiceProxy("/teach_path_generate",GenerateTeachPath)
        self.__task_record_client = rospy.ServiceProxy("/taskgroup_record_server",TaskReportManager)
        #发布
        self.__onlinetask_ctrl_pub = rospy.Publisher("/onlinetask_ctrl",OnlineTaskCtrl,queue_size=255)
        #订阅
        self.onlinetask_sub = rospy.Subscriber("/online_tasks_deque", OnlineTask, self.OnlineTask_Callback,queue_size=1)
        self.taskecho_sub = rospy.Subscriber("/task_echo_v3", TaskEcho, self.Taskecho_Callback,queue_size=1)

    def InitParser(self, node_name):
        pass


    def ResDataInit(self):
        '''
            初始化返回数据
        '''
        return {
            "code": 0,
            "msg": "success",
        }

    def ResDataError(self, msg = "failed"):
        '''
            返回异常数据
        '''
        return {
            "code": -1,
            "msg": msg,
        }

    def Taskecho_Callback(self,msg):
        """ 
            实时任务执行信息回调函数
        """
        self.__taskecho = msg

    def OnlineTask_Callback(self,msg):
        """ 
            实时任务执行信息回调函数
        """
        self.__online_tasks_deque = msg

    def IsRobotFree(self):
        '''
            判断任务队列是否为空
        '''
        return len(self.__online_tasks_deque.taskgroup_list) == 0
    def CleanModeParser(self, mode_array):
        '''
            解析清扫模式
        '''
        def GenPathType(type):
            '''
                生成路径类型
            '''
            path_type = PathGenerateType()
            path_type.type = type
            return path_type

        path_types = list()
        control_mode = TaskControlMode()
        if len(mode_array) == 3 :
            mode_array.append(0)
        # mode[0] 普通; mode[1] 贴边; mode[2] 补漏; mode[3] 0不选择路径,1:弓字形,2:回字形,3:路径自适应; 
        if mode_array[0]:
            if mode_array[1]:   #标准 + 贴边
                control_mode.mode = TaskControlMode.SWEEP_STANDARD_AND_EDGE
                print("SWEEP_STANDARD_AND_EDGE")
                if mode_array[3] == 1:   #弓字形
                    path_types.append(GenPathType(PathGenerateType.BOUSTROPHEDON_METHOD))
                    path_types.append(GenPathType(PathGenerateType.EDGE_METHOD))
                    print("BOUSTROPHEDON_METHOD")
                if mode_array[3] == 2:   #回字形
                    path_types.append(GenPathType(PathGenerateType.WRAPPER_METHOD))
                    path_types.append(GenPathType(PathGenerateType.EDGE_METHOD))
                    print("WRAPPER_METHOD")
            else:   #标准
                control_mode.mode = TaskControlMode.SWEEP_STANDARD_MODE
                print("SWEEP_STANDARD_MODE")
                if mode_array[3] == 1:   #弓字形
                    path_types.append(GenPathType(PathGenerateType.BOUSTROPHEDON_METHOD))
                    print("BOUSTROPHEDON_METHOD")
                if mode_array[3] == 2:   #回字形
                    path_types.append(GenPathType(PathGenerateType.WRAPPER_METHOD))
                    print("WRAPPER_METHOD")
        else:
            if mode_array[1]:   #贴边
                print("SWEEP_EDGE_MODE")
                control_mode.mode = TaskControlMode.SWEEP_EDGE_MODE
                path_types.append(GenPathType(PathGenerateType.EDGE_METHOD))
        return control_mode, path_types


    def PreviewCleanTaskPath(self, data):
        '''
           预览清扫任务路径
        '''
        ret_dat = self.ResDataInit()
        try:
            if not data : raise Exception("Request data error!")
            print(data)
            if "map_name" not in data : 
                raise Exception("map_name is empty!")
            map_name = data["map_name"].encode('utf-8')
            task_base = TaskBase()
            task_base.map_base.name = map_name

            # 起点
            if "path_start" not in data : 
                raise Exception("path_start is empty!")
            task_base.start.position.x = data["path_start"]["x"]
            task_base.start.position.y = data["path_start"]["y"]

            # 多边形轮廓
            if "origin" not in data : 
                raise Exception("origin is empty!")
            origin_points = data["origin"]
            # 清扫区域定点不得小于3个
            if len(origin_points) < 3 : 
                raise Exception("Invaild cleaning area!")
            for origin_point in origin_points:
                pose = Pose()
                pose.position.x = origin_point["x"]
                pose.position.y = origin_point["y"]
                task_base.polygon.poses.append(pose)
            
            # 控制模式
            task_base.task_type.type = TaskType.SWEEP_TASK
            clean_mode = data["mode"]
            print("clean_mode: {}".format(clean_mode))
            task_base.control_mode, task_base.path_gen.path_types = self.CleanModeParser(clean_mode)
            print(task_base.path_gen.path_types)
            # 请求预览路径
            req = TaskGenerateManagerRequest()
            req.option = req.PREVIEW_TASK
            req.task_base = task_base
            resp = self.__task_generate_client.call(req)
            if not resp.response.success:
                raise Exception(resp.response.message)
            
            # 返回预览路径
            ret_dat["origin"] = list()  # 轮廓数据
            res_task_base = resp.task_single.task_base
            for pose in res_task_base.polygon.poses:
                point_one = {
                    "x" : pose.position.x,
                    "y" : pose.position.y
                }
                ret_dat["origin"].append(point_one)

            ret_dat["sub_tasks"] = list()   # 路径信息
            # 常规路径
            for task_path in res_task_base.paths:
                point_dict = {
                    "path" : []
                }
                for pose in task_path.poses:
                    point_one = {
                        "x" : pose.position.x,
                        "y" : pose.position.y
                    }
                    point_dict["path"].append(point_one)
                ret_dat["sub_tasks"].append(point_dict)
            # 贴边路径
            for task_path in res_task_base.edge_paths:
                point_dict = {
                    "path" : []
                }
                for pose in task_path.poses:
                    point_one = {
                        "x" : pose.position.x,
                        "y" : pose.position.y
                    }
                    point_dict["path"].append(point_one)
                ret_dat["sub_tasks"].append(point_dict)  
        except Exception as error:
            rospy.logerr("[PreviewCleanTaskPath] error: ".format(error))
            ret_dat = self.ResDataError(str(error))
        finally:
            return ret_dat
    
    def CheckUtilsName(self, utils_ctrl_default_item_name):
        '''存储时，外设模式模式转换，默认值需存储为mode_1,mode_2,mode_4
        '''
        cleandevice_config_default_params = rospy.get_param("/cleandevice_config_default",{})
        for default_mode, value in cleandevice_config_default_params["translation"].items():
            if utils_ctrl_default_item_name in value:
                return default_mode
        return utils_ctrl_default_item_name 

    def ReadUtilsName(self, utils_ctrl_default_item_name):
        '''读取时，mode_1,mode_2,mode_4外设模式模式转换为当前语言下参数
        '''
        language = rospy.get_param("/language",0) #0 中文 1 英文 2 繁体中文 3 西班牙语 4 法语 5 日语 6 韩语 7 德语 
        cleandevice_config_default_params = rospy.get_param("/cleandevice_config_default",{})
        for default_mode, value in cleandevice_config_default_params["translation"].items():
            if utils_ctrl_default_item_name == default_mode:   #针对mode_1,mode_2,mode_4
                if language < len(value):
                    print("[ReadUtilsName] utils_ctrl_default_item_name: ",utils_ctrl_default_item_name, value[language])
                    return value[language]
        return utils_ctrl_default_item_name     

    def Task_add(self, data):
        '''
           任务操作，暂定增删改查
        '''
        ret_dat = self.ResDataInit()
        try:
            if not data : raise Exception("Request data error!")
            print(data)
            if "map_name" not in data: 
                raise Exception("map_name is empty!")
            if "task_name" not in data: 
                raise Exception("task_name is empty!") 
            map_name = data["map_name"].encode('utf-8')
            task_name = data["task_name"].encode('utf-8')

            task_base = TaskBase()
            task_base.map_base.name = map_name
            task_base.task_name = task_name

            # 起点
            if "path_start" not in data : 
                raise Exception("path_start is empty!")
            task_base.start.position.x = data["path_start"]["x"]
            task_base.start.position.y = data["path_start"]["y"]

            # 多边形轮廓
            if "origin" not in data : 
                raise Exception("origin is empty!")
            origin_points = data["origin"]
            # 清扫区域定点不得小于3个
            if len(origin_points) < 3 : 
                raise Exception("Invaild cleaning area!")
            for origin_point in origin_points:
                pose = Pose()
                pose.position.x = origin_point["x"]
                pose.position.y = origin_point["y"]
                task_base.polygon.poses.append(pose)
            
            # 控制模式
            # if "task_type" in data : 
            #     print(data["task_type"])
            #     task_base.task_type.type = data["task_type"]
            # else:
            #     task_base.task_type.type = TaskType.SWEEP_TASK
            task_base.task_type.type = TaskType.SWEEP_TASK
            clean_mode = data["mode"]
            print("clean_mode: {}".format(clean_mode))
            task_base.control_mode, task_base.path_gen.path_types = self.CleanModeParser(clean_mode)

            #路径是否静态，1：静态路径 0：动态路径每次执行计算路径
            task_base.dynamic_adjust_path = data["task_is_static"] == 0

            # 外设控制模式
            if "utils_ctrl_default_item_name" in data:
                task_base.utils_mode = self.CheckUtilsName(data["utils_ctrl_default_item_name"].encode('utf-8'))

            #指定任务执行速度
            clean_speed = int(data["clean_speed"])
            if clean_speed == 1:
                task_base.task_speed.speed = task_base.task_speed.LOW_SPEED
            if clean_speed == 2:
                task_base.task_speed.speed = task_base.task_speed.MIDDLE_SPEED
            if clean_speed == 3:
                task_base.task_speed.speed = task_base.task_speed.HIGH_SPEED

            # 请求保存路径
            req = TaskGenerateManagerRequest()
            req.option = req.CREATE_TASK
            req.task_base = task_base
            resp = self.__task_generate_client.call(req)
            if not resp.response.success:
                raise Exception(resp.response.message)
            # tasksingle = TaskSingle()
            # tasksingle.task_base = task_base
            # #生成任务组  任务管理节点自动保存一份任务组数据
            # task_group = TaskGroup()
            # task_group.group_name = task_name
            # task_group.task_list.append(tasksingle)
            # # task_group.task_list.append(task_single)
            # # task_group.timertask_name = timer_task.timertask_name

            # req_group = TaskGroupManagerRequest()
            # req_group.task_group = task_group
            # req_group.option = req_group.CREATE_TASK_GROUP
            # res_group = self.__grouptask_manager_client.call(req_group)
   
        except Exception as error:
            rospy.logerr("[Task_manager] error: ".format(error))
            ret_dat = self.ResDataError(str(error))
        finally:
            return ret_dat
    


    
    def Task_delet(self, data):
        '''
           任务操作，删除
        '''
        ret_dat = self.ResDataInit()
        try:
            if not data : raise Exception("Request data error!")
            print(data)
            if "map_name" not in data: 
                raise Exception("map_name is empty!")
            if "task_name" not in data: 
                raise Exception("task_name is empty!") 
            map_name = data["map_name"].encode('utf-8')
            task_name = data["task_name"].encode('utf-8')

            task_single = TaskSingle()
            task_single.task_base.task_name = task_name
            task_single.task_base.map_base.name = map_name

            # 单个任务删除请求
            req = TaskSingleManagerRequest()
            req.option = req.DELETE_TASK
            req.task_single = task_single
            resp = self.__task_single_client.call(req)
            if not resp.response.success:
                raise Exception(resp.response.message)
   
        except Exception as error:
            rospy.logerr("[Task_manager] error: ".format(error))
            ret_dat = self.ResDataError(str(error))
        finally:
            return ret_dat

    def Task_rename(self, data):
        '''
           任务操作，重命名
        '''
        ret_dat = self.ResDataInit()
        try:
            if not data : raise Exception("Request data error!")
            print(data)
            if "map_name" not in data: 
                raise Exception("map_name is empty!")
            if "task_name" not in data: 
                raise Exception("task_name is empty!") 
            if "task_name_new" not in data: 
                raise Exception("task_name_new is empty!") 
            map_name = data["map_name"].encode('utf-8')
            task_name = data["task_name"].encode('utf-8')
            task_name_new = data["task_name_new"].encode('utf-8')

            task_single = TaskSingle()
            task_single.task_base.map_base.name = map_name
            task_single.task_base.task_name = task_name

            task_base = TaskBase()
            task_base.map_base.name = map_name
            task_base.task_name = task_name_new
            #指定任务执行速度
            clean_speed = int(data["clean_speed"])
            if clean_speed == 1:
                task_base.task_speed.speed = task_base.task_speed.LOW_SPEED
            if clean_speed == 2:
                task_base.task_speed.speed = task_base.task_speed.MIDDLE_SPEED
            if clean_speed == 3:
                task_base.task_speed.speed = task_base.task_speed.HIGH_SPEED
            # 清扫模式
            clean_mode = data["mode"]
            print("clean_mode: {}".format(clean_mode))
            task_base.control_mode, task_base.path_gen.path_types = self.CleanModeParser(clean_mode)
            # 外设控制模式
            if "utils_ctrl_default_item_name" in data:
                task_base.utils_mode = self.CheckUtilsName(data["utils_ctrl_default_item_name"].encode('utf-8'))

            task_single_new = TaskSingle()
            task_single_new.task_base = task_base        
            # 单个任务重命名
            req = TaskSingleManagerRequest()
            req.option = req.UPDATE_TASK
            req.task_single = task_single
            req.new_task_single = task_single_new
            resp = self.__task_single_client.call(req)
            if not resp.response.success:
                raise Exception(resp.response.message)
   
        except Exception as error:
            rospy.logerr("[Task_rename] error: {}".format(error))
            ret_dat = self.ResDataError(str(error))
        finally:
            return ret_dat


    def Task_list(self, data):
        '''
           当前地图下任务列表查询
        '''
        ret_dat = self.ResDataInit()
        try:
            if not data : raise Exception("Request data error!")
            # print(data)
            if "map_name" not in data: 
                raise Exception("map_name is empty!")
            map_name = data["map_name"].encode('utf-8')
            task_single = TaskSingle()
            task_single.task_base.map_base.name = map_name
            # print(task_single.task_base.map_base.name)
            # 当前地图下任务列表查询
            req = TaskSingleManagerRequest()
            req.option = req.QUERY_TASK_INFO_WITH_MAP
            req.task_single = task_single
            resp = self.__task_single_client.call(req)
            if not resp.response.success:
                raise Exception(resp.response.message)
            print("[Task_list] task_list size: ", len(resp.task_list))
            tasks=[]
            for i in range(len(resp.task_list)):
                task_one_info={}
                print(resp.task_list[i].task_base.task_name)
                task_one_info["task_name"] = resp.task_list[i].task_base.task_name
                task_one_info["note"] = ""

                # 速度显示异常，需要转换
                if resp.task_list[i].task_base.task_speed.speed == TaskSpeed.LOW_SPEED:
                    task_one_info["global_spd"] = 1
                if resp.task_list[i].task_base.task_speed.speed == TaskSpeed.MIDDLE_SPEED:
                    task_one_info["global_spd"] = 2
                if resp.task_list[i].task_base.task_speed.speed == TaskSpeed.HIGH_SPEED:
                    task_one_info["global_spd"] = 3
                    

                # 添加清洁外设的模式 20230905-dw.liu
                # task_one_info["utils_ctrl_default_item_name"] = task_query_res.clean_task.clean_mode_name
                task_one_info["utils_ctrl_default_item_name"] = self.ReadUtilsName(resp.task_list[i].task_base.utils_mode)
                                # 添加清扫任务起点
                path_start_info = {}
                path_start_point = resp.task_list[i].task_base.start
                path_start_info = {
                        "x":path_start_point.position.x,
                        "y":path_start_point.position.y
                    }
                task_one_info["path_start"] = path_start_info
                raw_task = resp.task_list[i]
                clean_mode = raw_task.task_base.control_mode.mode
                path_type = raw_task.task_base.paths
                
                # mode[0] 普通; mode[1] 贴边; mode[2] 补漏; mode[3] 0不选择路径,1:弓字形,2:回字形,3:路径自适应; 
           
                # int16 UNKNOWN = 100                   #未知
                # int16 SWEEP_STANDARD_MODE = 0         #标准清扫模式
                # int16 SWEEP_EDGE_MODE = 10            #贴边清扫模式
                # int16 SWEEP_STANDARD_AND_EDGE = 11    #标准+贴边清扫模式
                # int16 SWEEP_STANDARD_AND_REPAIR = 21  #标准+补漏清扫模式
                # int16 FOLLOW_MODE = 31                #跟随模式  
                mode_info = [0,0,0,0]
                # NORM_SWEEP 清扫（普通弓形）; NROM_PERI 清扫（贴边+普通弓形）
                # if clean_mode == raw_task.task_base.control_mode.SWEEP_STANDARD_MODE:
                #     mode_info[0] = 1
                # if clean_mode == raw_task.task_base.control_mode.SWEEP_EDGE_MODE:
                #     mode_info[1] = 1
                # if clean_mode == raw_task.task_base.control_mode.SWEEP_STANDARD_AND_EDGE:
                #     mode_info[0] = 1
                #     mode_info[1] = 1
                # if clean_mode == raw_task.task_base.control_mode.SWEEP_STANDARD_AND_REPAIR: 
                #     mode_info[0] = 1
                #     mode_info[2] = 1
                control_mode = raw_task.task_base.control_mode.mode
                if control_mode == TaskControlMode.SWEEP_STANDARD_AND_EDGE:
                    mode_info[0] = 1
                    mode_info[1] = 1
                if control_mode == TaskControlMode.SWEEP_STANDARD_MODE:
                    mode_info[0] = 1
                if control_mode == TaskControlMode.SWEEP_EDGE_MODE:
                    mode_info[1] = 1

                print("type", raw_task.task_base.task_type.type)
                print("path_types", raw_task.task_base.path_gen.path_types)
                if raw_task.task_base.task_type.type == TaskType.SWEEP_TASK: 
                    if raw_task.task_base.path_gen.path_types:
                        if raw_task.task_base.path_gen.path_types[0].type == PathGenerateType.BOUSTROPHEDON_METHOD:
                            mode_info[3] = 1
                        if raw_task.task_base.path_gen.path_types[0].type == PathGenerateType.WRAPPER_METHOD:
                            mode_info[3] = 2
                if raw_task.task_base.task_type.type == TaskType.TEACH_TASK:
                    if raw_task.task_base.path_gen.path_types_child:
                        if raw_task.task_base.path_gen.path_types_child[0].type == PathGenerateType.BOUSTROPHEDON_METHOD:
                            mode_info[3] = 1
                        if raw_task.task_base.path_gen.path_types_child[0].type == PathGenerateType.WRAPPER_METHOD:
                            mode_info[3] = 2
                print("mode_info", mode_info)

                #待添加逻辑
                # type 1 划区清扫 2 示教区域  3 示教路线
                task_one_info["type"] = 1
                if raw_task.task_base.task_type.type == TaskType.SWEEP_TASK:
                    task_one_info["type"] = 1
                if raw_task.task_base.task_type.type == TaskType.TEACH_TASK:
                    if raw_task.task_base.path_gen.path_types:
                        if raw_task.task_base.path_gen.path_types[0].type == PathGenerateType.MANUAL_POLYGON_METHOD:
                            task_one_info["type"] = 2
                        if raw_task.task_base.path_gen.path_types[0].type == PathGenerateType.MANUAL_METHOD:
                            task_one_info["type"] = 3    
                task_one_info["mode"] = mode_info       

                #区域路径
                origin_info = []
                for pose in raw_task.task_base.polygon.poses:
                    point_one = {
                            "x":pose.position.x,
                            "y":pose.position.y
                        }
                    origin_info.append(point_one)
                task_one_info["origin"] = origin_info
                # 常规路径
                task_one_info["sub_tasks"] = []
                for task_path in raw_task.task_base.paths:
                    point_dict = {
                        "path" : []
                    }
                    for pose in task_path.poses:
                        point_one = {
                            "x" : pose.position.x,
                            "y" : pose.position.y
                        }
                        point_dict["path"].append(point_one)
                    task_one_info["sub_tasks"].append(point_dict)
                # 贴边路径
                for task_path in raw_task.task_base.edge_paths:
                    point_dict = {
                        "path" : []
                    }
                    for pose in task_path.poses:
                        point_one = {
                            "x" : pose.position.x,
                            "y" : pose.position.y
                        }
                        point_dict["path"].append(point_one)
                    task_one_info["sub_tasks"].append(point_dict)
                task_one_info["sub_tasks_img_url"]=""
                tasks.append(task_one_info)
            ret_dat["task_list"] = tasks
        except Exception as error:
            rospy.logerr("[Task_list] error: ".format(error))
            ret_dat = self.ResDataError(str(error))
        finally:
            return ret_dat

    def TaskRegenerate(self,data):
        '''
            清扫任务重新规划
            实现思路：
            循环
            1.任务重新规划的前提条件，任务地图须是当前加载的地图
            2.获取地图下的所有任务的原始信息，获取任务规划信息
            3.删除当前任务
            4.添加新规划任务
        '''
        ret_dat = self.ResDataInit()
        try:
            if not data: raise Exception("Request data error!")
            map_name = data["map_name"].encode('utf-8')
            if not map_name: raise Exception("map_name is empty!")

            # 加载当前地图
            if self.__map_name != map_name:
                raise Exception("Please load this map! ")
            
            # 获取当前地图的任务列表
            task_list_req = DiskTaskListRequest()
               
            task_list_req.map_name = map_name   
            task_list_req.global_ = 0
            map_task_data = {}     
            res_data = self.__task_list_query_client(task_list_req)

            if not res_data.names:
                raise Exception("The mission on the current map is empty!")
            
            #根据任务name获取任务数据
            for name in reversed(res_data.names):
                task_req = TaskQueryRequest()
                task_req.id = 0
                task_req.task_name = name
                task_req.map_name = map_name
                task_query_res = self.__task_query_client(task_req)
                # 成功获取到数据
                if task_query_res.success:

                    # 任务原始数据，重新规划和添加

                    ## 获取任务路径数据
                    raw_task = CleanTask()
                    raw_task.task_name =  task_query_res.clean_task.task_name
                    raw_task.descript = task_query_res.clean_task.descript
                    raw_task.map_name = task_query_res.clean_task.map_name
                    raw_task.clean_mode = task_query_res.clean_task.clean_mode
                    raw_task.type = task_query_res.clean_task.type
                    raw_task.global_spd = task_query_res.clean_task.global_spd
                    raw_task.path_start = task_query_res.clean_task.path_start
                    raw_task.global_area = task_query_res.clean_task.global_area
                    raw_task.clean_mode_name = task_query_res.clean_task.clean_mode_name  

                    # print(raw_task)

                    #缓存任务
                    task = raw_task

                    # 根据刘洋提供的服务适配修改 20230530-dw.liu
                    raw_task.state = raw_task.FABRICATING
                    packed_req = CoverPathGenRequest()
                    packed_req.raw = raw_task
                    packed_req.plan_area_type = packed_req.CUSTOMIZATION # 自定义规划区域（通过APP规划区域)
                    res = self.__task_all_sparse_path_client.call(packed_req)

                    print(res)
                    # if not res.success: raise Exception("service /all_sparse_path error: {}".format(res.message))
                    if res.success:
                        task = res.packed
                        self.__task_update_pub.publish(task)

            log_msg = "[TaskParser]Task Regenerate  map<{}>".format(map_name)
            log.LogInfo(self.__node_name,log_msg)
        except Exception as error:

            ret_dat["code"]=-1
            ret_dat["msg"]=str(error)

            err_msg = "[TaskParser]Failed to Task Regenerate : {}".format(str(error))
            log.LogError(self.__node_name,err_msg)
            ret_dat = self.ResDataError(str(error))

        finally:
            return ret_dat


    def AddTimertask(self,data):
        '''
            定时任务添加
        '''    
        print(data)
        #ret_dat = {};ret_dat["ret"]='success';ret_dat["code"]=0 
        ret_dat = self.ResDataInit()
        try:
            if not data: raise Exception("Request data error!")
            
            if not data["map_name"]: raise Exception("map_name is empty!")
            map_name = data["map_name"].encode('utf-8')
            
            if not data["timetask_name"]: raise Exception("timetask_name is empty!")
            timetask_name = data["timetask_name"].encode('utf-8')
            
            if not data["start_time"]: raise Exception("start_time is empty!")
            start_time = data["start_time"].encode('utf-8')

            timer_task = TimerTask()
            timer_task.multi_maps_shared = False
            timer_task.map_name = map_name
            timer_task.timertask_name = timetask_name
            timer_task.enable = bool(data["enable"])
            timer_task.timetable = data["days"]
            timer_task.start_time = start_time
            for cleantask_one in data["timetask_list"]:
                # print(cleantask_one)
                ##预防空任务名
                if not cleantask_one: continue
                #包装成组合任务
                # task_single = TaskSingle()
                # task_single.task_base.task_name = cleantask_one.encode('utf-8')
                # task_single.task_base.map_base.name = map_name
                # task_single.task_base.group_name = cleantask_one.encode('utf-8')
            
                task_group = TaskGroup()
                task_group.group_name = cleantask_one.encode('utf-8')
                # task_group.task_list.append(task_single)
                task_group.timertask_name = timer_task.timertask_name

                # req_group = TaskGroupManagerRequest()
                # req_group.task_group = task_group
                # req_group.option = req_group.CREATE_TASK_GROUP
                # res_group = self.__grouptask_manager_client.call(req_group)
                # if not res_group.response.success:
                #     raise Exception(res_group.response.message)
                timer_task.taskgroup_list.append(task_group)


            req=TimerTaskManagerRequest()
            req.option=req.CREATE_TIMER_TASK
            req.timertask = timer_task
            res = self.__timertask_manager_client.call(req)
            if not res.response.success:
                raise Exception(res.response.message)
            
            log_msg = "[TaskParser]Add timer task<{}> in map<{}>".format(timetask_name, map_name)
            log.LogInfo(self.__node_name,log_msg)
        except Exception as error:
            err_msg = "[TaskParser]Failed to add timer task: {}".format(error)
            log.LogError(self.__node_name,err_msg)
            ret_dat = self.ResDataError(str(error))
            
            rospy.logerr(err_msg)

            #ret_dat["ret"]='fail'
            ret_dat["code"]=-1
            ret_dat["msg"]=str(error)
        finally:   
            return ret_dat
            
    def ListTimertask(self,data):
        '''
            查询定时任务信息
            改为：传地图名，查询当前地图下的定时任务，没有地图名，则是全部定时任务
        '''    
        ret_dat = self.ResDataInit() 
        try:
            if not data: raise Exception("Request data error!")

            if data.has_key("map_name"):
                if data["map_name"]:
                    map_name = data["map_name"].encode('utf-8')
                else:
                    map_name = None
            else:
                map_name = None

            req=TimerTaskManagerRequest()
            req.option=req.QUERY_ALL_TIMER_TASK_INFO
            # req.timertask = timer_task
            res = self.__timertask_manager_client.call(req)
            if not res.response.success:
                raise Exception(res.response.message)
            timetask_info = []
            for task_one in res.timertask_list:
                task_names = []
                for group_task in task_one.taskgroup_list:
                    task_names.append(group_task.group_name)
                task_one_info = {
                    "map_name": task_one.map_name,
                    "timetask_name": task_one.timertask_name,
                    "enable": task_one.enable,
                    "timetask_list": task_names,
                    "days": task_one.timetable,
                    "start_time": task_one.start_time,
                    "create_date": "",
                    "create_date_sort": task_one.start_time # 任务创建时间用于排序
                }
                print(task_one_info)
                timetask_info.append(task_one_info)
            
        
            # ret_dat["timetask_info"]=timetask_info
            ret_dat["timetask_info"] = sorted(timetask_info,key=lambda task_one_info:task_one_info['create_date_sort'],reverse=True)
            log_msg = "[TaskParser][ListTimertask]Successful to list timer task."
            log.LogInfo(self.__node_name,log_msg)
        except Exception as error:
            err_msg = "[TaskParser][ListTimertask]Failed to list timer task: {}".format(error)
            log.LogError(self.__node_name,err_msg)
            ret_dat = self.ResDataError(str(error))
            
            # # rospy.logerr(err_msg)

            # ret_dat["code"]=-1
            # ret_dat["msg"]=str(error)
        finally:   
            return ret_dat


    def CancelTimertask(self,data):
        print(data)
        '''
            定时任务开关
        '''    
        ret_dat = self.ResDataInit()    
        try:
            if not data:
                raise Exception("Request data error!")

            # if not ["map_name"]: raise Exception("map_name is empty!")
            # map_name = data["map_name"].encode('utf-8')

            if not data["timetask_name"]:
                raise Exception("timetask_name is empty!")
            timetask_name = data["timetask_name"].encode('utf-8')
            
            enable = bool(data["enable"])
            req=TimerTaskManagerRequest()
            req.option=req.QUERY_TIMER_TASK_INFO
            req.timertask.timertask_name = timetask_name
            res = self.__timertask_manager_client.call(req)
            if not res.response.success:
                raise Exception(res.response.message)
            
            timer_task = TimerTask()
            for time_task in res.timertask_list:
                timer_task = time_task

            print(timer_task.enable, enable)
            if timer_task.enable != enable:
                timer_task.enable = enable
                req_update=TimerTaskManagerRequest()
                req_update.option=req_update.UPDATE_TIMER_TASK
                old_timer_task = TimerTask()
                old_timer_task.timertask_name = timetask_name
                req_update.timertask = old_timer_task
                req_update.new_timertask = timer_task
                res = self.__timertask_manager_client.call(req_update)
                if not res.response.success:
                    raise Exception(res.response.message)
                ##记录
                log_msg = "[TaskParser]Switch timer task<{}> to <{}>".format(timetask_name, enable)
                log.LogInfo(self.__node_name,log_msg)
        except Exception as error:
            err_msg = "[TaskParser]Failed to cancel timer task: {}".format(error)
            log.LogError(self.__node_name,err_msg)
            
            rospy.logerr(err_msg)
            ret_dat = self.ResDataError(str(error))
        finally:   
            return ret_dat

    def ModifyTimertask(self,data):
        '''
            定时任务编辑
        '''    
        ret_dat =self.ResDataInit()       
        print(data)
        try:
            if not data: raise Exception("Request data error!")

            # if not data["map_name"]: raise Exception("map_name is empty!")
            # map_name = data["map_name"].encode('utf-8')

            if not data["timetask_name"]: raise Exception("timetask_name is empty!")
            timetask_name = data["timetask_name"].encode('utf-8')
            
            timer_task = TimerTask()
            timer_task.timertask_name = timetask_name

            new_timer_task = TimerTask()
            # new_timer_task.map_name = "map_name"
            new_timer_task.timertask_name = timetask_name
            new_timer_task.enable = bool(data["enable"])
            new_timer_task.timetable = data["days"]
            new_timer_task.start_time = data["start_time"]
            for cleantask_one in data["timetask_list"]:
                ##预防空任务名
                if not cleantask_one: continue
                task_group = TaskGroup()
                task_group.group_name = cleantask_one.encode('utf-8')
                new_timer_task.taskgroup_list.append(task_group)
            req_update=TimerTaskManagerRequest()
            req_update.option=req_update.UPDATE_TIMER_TASK
            req_update.timertask = timer_task
            req_update.new_timertask = new_timer_task
            res = self.__timertask_manager_client.call(req_update)
            if not res.response.success:
                raise Exception(res.response.message)
            ##记录
            log_msg = "[TaskParser]Modify timer task<{}>>".format(timetask_name)
            log.LogInfo(self.__node_name,log_msg)
        except Exception as error:
            err_msg = "[TaskParser]Failed to modify timer task: {}".format(error)
            log.LogError(self.__node_name,err_msg)
            rospy.logerr(err_msg)

            ret_dat = self.ResDataError(str(error))
        finally:   
            return ret_dat


    def DeleteTimertask(self,data):
        '''
            定时任务删除
        '''    
        ret_dat = self.ResDataInit()     
        try:
            if not data: raise Exception("Request data error!")

            # map_name = data["map_name"].encode('utf-8')
            # if not map_name: raise Exception("map_name is empty!")

            timetask_name = data["timetask_name"].encode('utf-8')
            if not timetask_name: raise Exception("timetask_name is empty!")

            timer_task = TimerTask()
            timer_task.timertask_name = timetask_name

            ## 删除定时任务
            req=TimerTaskManagerRequest()
            req.option=req.DELETE_TIMER_TASK
            req.timertask = timer_task
            res = self.__timertask_manager_client.call(req)
            if not res.response.success:
                raise Exception(res.response.message)

            ##记录
            log_msg = "[TaskParser]Delete timer task<{}>".format(timetask_name)
            log.LogInfo(self.__node_name,log_msg)
        except Exception as error:
            err_msg = "[TaskParser]Failed to delete timer task: {}".format(error)
            log.LogError(self.__node_name,err_msg)
            
            rospy.logerr(err_msg)

            ret_dat = self.ResDataError(str(error))
        finally:   
            return ret_dat

    def RenameTimertask(self,data):
        '''
            定时任务重命名
        '''    
        ret_dat = self.ResDataInit()
        try:
            if not data: raise Exception("Request data error!")

            # if not data["map_name"]: raise Exception("map_name is empty!")
            # map_name = data["map_name"].encode('utf-8')
            
            if not data["timetask_oldname"]: raise Exception("timetask_oldname is empty!")
            timetask_name = data["timetask_oldname"].encode('utf-8')
            
            if not data["timetask_newname"]: raise Exception("timetask_newname is empty!")
            timetask_newname = data["timetask_newname"].encode('utf-8')
             
            #比较一致性
            if timetask_name != timetask_newname:
                new_timer_task = TimerTask()
                timer_task = TimerTask()
                req_update=TimerTaskManagerRequest()
                req_update.option=req_update.UPDATE_TIMER_TASK
                timer_task.timertask_name = timetask_name
                new_timer_task.timertask_name = timetask_newname
                req_update.timertask = timer_task
                req_update.new_timertask = new_timer_task
                res = self.__timertask_manager_client.call(req_update)
                if not res.response.success:
                    raise Exception(res.response.message)
                
                ##记录
                log_msg = "[TaskParser]Rename timer task from <{}> to <{}>".format(timetask_name,timetask_newname)
                log.LogInfo(self.__node_name,log_msg)
        except Exception as error:
            err_msg = "[TaskParser]Failed to rename timer task: {}".format(error)
            log.LogError(self.__node_name,err_msg)
            
            rospy.logerr(err_msg)

            #ret_dat["ret"]='fail'
            ret_dat = self.ResDataError(str(error))
        finally:   
            return ret_dat

    def StartCleanTask(self,data):
        '''
            清扫任务开始执行
        '''
        ret_dat = self.ResDataInit()  
        try:
            if not data: raise Exception("Request data error!")
            print(data)
            for task_one in data:
                map_name = task_one["map_name"].encode('utf-8')
                if not map_name: Exception("map_name is empty!")

                task_name = None
                scheduled_task_name = None
                
                if "task_name" in task_one:
                    task_name = task_one["task_name"].encode('utf-8')
                if "scheduled_task_name" in task_one:
                    scheduled_task_name = task_one["scheduled_task_name"].encode('utf-8') # 定时任务名称
                
                if not task_name and not scheduled_task_name: Exception("task_name is empty!")
                print(task_name)    
                if task_name:
                    onlintaskctrl = OnlineTaskCtrl()
                    onlintaskctrl.group_name = task_name
                    onlintaskctrl.option = onlintaskctrl.TASK_START
                    onlintaskctrl.send_node_name = self.__node_name
                    self.__onlinetask_ctrl_pub.publish(onlintaskctrl) 
                    log_msg = "[TaskParser]Add task<{}> to task queue in map<{}>".format(task_name, map_name)
                    log.LogInfo(self.__node_name,log_msg)
                print(scheduled_task_name)
                if scheduled_task_name:
                    req=TimerTaskManagerRequest()
                    req.option=req.QUERY_TIMER_TASK_INFO
                    req.timertask.timertask_name = scheduled_task_name
                    res = self.__timertask_manager_client.call(req)
                    print(res)
                    if not res.response.success:
                        raise Exception(res.response.message)
                    for time_task in res.timertask_list:
                        for task in time_task.taskgroup_list:
                            time.sleep(0.01)
                            onlintaskctrl = OnlineTaskCtrl()
                            onlintaskctrl.group_name = task.group_name
                            onlintaskctrl.option = onlintaskctrl.TASK_START
                            onlintaskctrl.send_node_name = self.__node_name
                            self.__onlinetask_ctrl_pub.publish(onlintaskctrl) 
                    log_msg = "[TaskParser]Add task<{}> to task queue".format(task_name)
                    log.LogInfo(self.__node_name,log_msg)
                # if  elevator_scheduled_task_name:
                #     client_data = ElevatorTimerTaskManagerRequest() 
                #     client_data.state = client_data.TIMERTASK_QUERY # 梯控定时任务查询
                    
                #     res_data = self.__elevator_timertask_query_client.call(client_data)
                #     if not res_data.success :raise Exception("service</elevator_timertask_query> error: {}".format(res_data.message))
                #     # print(res_data)
                #     timer_tasks = res_data.timertask_list # 梯控定时任务列表
                #     # print(timer_tasks)
                #     for task_one in timer_tasks.timertask_list:
                #         # print(task_one)
                #         # print(task_one.map_names)
                #         if elevator_scheduled_task_name == task_one.name:
                #             for index,map_name_value in enumerate(task_one.map_names):
                #                 # print(index)
                #                 # print(map_name_value)
                            
                #                 ##从数据库获取任务信息
                #                 task_req = TaskQueryRequest()
                #                 task_req.id = 0
                #                 task_req.task_name = task_one.task_names[index]
                #                 task_req.map_name = map_name_value
                #                 task_query_res = self.__task_query_client(task_req)
                                
                #                 ##请求失败处理
                #                 if not task_query_res.success:
                #                     raise Exception("Failed to query task by name<{}>: {}".format(task_name,task_query_res.message))
                                
                #                 clean_task = CleanTask()
                                
                #                 clean_task = task_query_res.clean_task
                                
                #                 if utils_ctrl_default_item_name :                   # 不为空 清扫任务添加清洁模式 20230824-dw.liu
                #                     clean_task.clean_mode_name = utils_ctrl_default_item_name

                #                 self.PublishTaskAddTopic(clean_task) 

                #                 log_msg = "[TaskParser]Add task<{}> to task queue in map<{}>".format(task_name, map_name)
                #                 log.LogInfo(self.__node_name,log_msg)
                    
                        
        except Exception as error:

            ret_dat = self.ResDataError(str(error))
        
            err_msg = "[TaskParser]Failed to start task: {}".format(str(error))
            log.LogError(self.__node_name,err_msg)
        finally:   
            return ret_dat
    

    def ListCleanTask(self,data):
        '''
            执行任务列表
        '''
        ret_dat = self.ResDataInit() 
        try:
            if not data: raise Exception("Request data error!")

            if data["map_name"]:
                map_name = data["map_name"].encode('utf-8')
            else:
                map_name = None
            # if not map_name: raise Exception("map_name is empty!")
            task_list = []
            for task in self.__online_tasks_deque.taskgroup_list:
                # if not map_name or task.map_name == map_name:
                task_info = { 
                                "cleantask_name":task.group_name,
                                "map_name":"",
                                "id": int(task.group_id)
                            }
                task_list.append(task_info)
            ret_dat["task_list"] =task_list
            log_msg = "[TaskParser]Successful to obtain task queue."
            log.LogInfo(self.__node_name,log_msg)
        except Exception as error:
            #ret_dat["ret"]='fail'
            ret_dat["code"]=-1
            ret_dat["msg"]=str(error)

            err_msg = "[TaskParser]Failed to obtain task queue: {}".format(str(error))
            log.LogError(self.__node_name,err_msg)
            rospy.logerr(err_msg)
        finally:
            return ret_dat
        
    def CancelOneCleanTask(self,data):
        '''
            清扫任务取消执行(单个任务)
        '''
        print(data)
        ret_dat = self.ResDataInit() 
        try:
            if not data: raise Exception("Request data error!")
            task_name = data["task_name"].encode('utf-8')
            task_id = str(data["id"])
            print(task_name,task_id)
            #后面可以写成函数
            onlinetask_ctrl = OnlineTaskCtrl()
            onlinetask_ctrl.group_id = task_id
            onlinetask_ctrl.send_node_name = self.__node_name
            onlinetask_ctrl.option = onlinetask_ctrl.TASK_CANCEL
            self.__onlinetask_ctrl_pub.publish(onlinetask_ctrl) 
            log_msg = "[TaskParser]Cancel task<name: {}, ID: {}> ".format(task_name,task_id)
            log.LogInfo(self.__node_name,log_msg)

        except Exception as error:
            #ret_dat["ret"]='fail'
            ret_dat["code"]=-1
            ret_dat["msg"]=str(error)

            err_msg = "[TaskParser]Failed to cancel task: {}".format(str(error))
            log.LogError(self.__node_name,err_msg)
            rospy.logerr(err_msg)
        finally:
            return ret_dat
    def CancelAllCleanTask(self):
        '''
            清扫任务取消执行(所有任务)
        '''
        ret_dat = self.ResDataInit()   
        try:
            #取消所有任务
            onlinetask_ctrl = OnlineTaskCtrl()
            onlinetask_ctrl.send_node_name = self.__node_name
            onlinetask_ctrl.option = onlinetask_ctrl.TASK_CANCEL_ALL
            self.__onlinetask_ctrl_pub.publish(onlinetask_ctrl) 
            log_msg = "[TaskParser]Cancel all task queue"
            log.LogInfo(self.__node_name,log_msg)

        except Exception as error:
            #ret_dat["ret"]='fail'
            ret_dat["code"]=-1
            ret_dat["msg"]=str(error)

            err_msg = "[TaskParser]Failed to cancel all task: {}".format(str(error))
            log.LogError(self.__node_name,err_msg)
            rospy.logerr(err_msg)
        finally:
            return ret_dat

    def PauseCleanTask(self):
        '''
            清扫任务暂停
        '''
        ret_dat = self.ResDataInit()   
        try:
            onlinetask_ctrl = OnlineTaskCtrl()
            onlinetask_ctrl.send_node_name = self.__node_name
            onlinetask_ctrl.option = onlinetask_ctrl.TASK_PAUSE
            self.__onlinetask_ctrl_pub.publish(onlinetask_ctrl) 
            log_msg = "[TaskParser]Sucuessful to pause task"
            log.LogInfo(self.__node_name,log_msg)
        except Exception as error:
            #ret_dat["ret"]='fail'
            ret_dat["code"]=-1
            ret_dat["msg"]=str(error)

            err_msg = "[TaskParser]Failed to pause task: {}".format(str(error))
            log.LogError(self.__node_name,err_msg)
            rospy.logerr(err_msg)
        finally:
            return ret_dat

    def ResumeCleanTask(self):
        '''
            清扫任务恢复
        '''
        ret_dat = self.ResDataInit()   
        try:
            onlinetask_ctrl = OnlineTaskCtrl()
            onlinetask_ctrl.send_node_name = self.__node_name
            onlinetask_ctrl.option = onlinetask_ctrl.TASK_RESUME
            self.__onlinetask_ctrl_pub.publish(onlinetask_ctrl)  
            log_msg = "[TaskParser]Sucuessful to resume task"
            log.LogInfo(self.__node_name,log_msg)

        except Exception as error:
            #ret_dat["ret"]='fail'
            ret_dat["code"]=-1
            ret_dat["msg"]=str(error)

            err_msg = "[TaskParser]Failed to resume task: {}".format(str(error))
            log.LogError(self.__node_name,err_msg)
            rospy.logerr(err_msg)
        finally:
            return ret_dat

    #待重写
    def OrderCleanTask(self,data):
        '''
            执行任务列表重新排序
        '''
        ret_dat = self.ResDataInit()   
        try:
            if not data: raise Exception("Request data error!")

            new_task_order = data
            print(new_task_order)
            
            for new_index in range(len(new_task_order)):
                origin_index = self.FindTaskIndexInTaskQueue(new_task_order[new_index])
                if origin_index < 0 : raise Exception("Task ID<{}> error!".format(new_task_order[new_index]))
                
                if new_index != origin_index:
                    success,msg = self.AdjustTaskOrderInTaskQueue(origin_index, new_index)
                    if not success : raise Exception("Failed to adjust task<{}> order: {}!".format(new_task_order[new_index],msg))
                    
                    ##等待新队列
                    now = time.time()
                    while True:
                        #等待10ms
                        time.sleep(0.01)

                        if new_task_order[new_index] == self.__realtime_task_queue.tasks[new_index].id:
                            break

                        #判断是否超时
                        if (time.time() - now) > self.__response_timeout:
                            raise Exception("response timeout!")
                        
                    self.PrintTaskQueueOrder()
                else:
                    print("---")
                
            log_msg = "[TaskParser]Sucuessful to adjust task order in map<{}>".format(self.__map_name)
            log.LogInfo(self.__node_name,log_msg)
        except Exception as error:
            #ret_dat["ret"]='fail'
            ret_dat["code"]=-1
            ret_dat["msg"]=str(error)

            err_msg = "[TaskParser]Failed to order task queue: {}".format(str(error))
            log.LogError(self.__node_name,err_msg)
            rospy.logerr(err_msg)
        finally:
            return ret_dat

    def GenPathGenerateType(self, data):
        path_type = PathGenerateType()
        path_type.type = data
        return path_type
    

    def StartTeachingPathRecord(self):
        '''
        开始示教路径记录
        '''
        print("StartTeachingPathRecord...................")
        ret_dat =self.ResDataInit()   
        try:
            # teaching_path_req = GenerateTeachPathRequest()
            # teaching_path_req.flag = teaching_path_req.START_RECOED
            # res_data = self.__teaching_path_client(teaching_path_req) 
            # if  res_data.success: raise Exception("service</teach_path_generate> error: {}".format(res_data.message))
            path_gen_msg = PathGenerate()
            path_gen_msg.path_types.append(
                self.GenPathGenerateType(PathGenerateType.MANUAL_METHOD)
            )
            path_gen_msg.path_action.data = PathGenerateAction.START_RECORD
            client_data = TaskGenerateManagerRequest()
            client_data.option = TaskGenerateManagerRequest.CREATE_TASK
            client_data.task_base.path_gen = path_gen_msg
            client_data.task_base.task_type.type = TaskType.TEACH_TASK
            client_data.task_base.control_mode.mode = TaskControlMode.SWEEP_EDGE_MODE
            resp = self.__task_generate_client.call(client_data)
            if not resp.response.success:
                raise Exception(resp.response.message)

            log_msg = "[TaskParser]Successful to start teaching path record."
            log.LogInfo(self.__node_name,log_msg)
        except Exception as error:
            rospy.logerr("[TaskParser]Failed to start teaching path record: {}".format(error))
            #ret_dat["ret"] = 'fail'
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[TaskParser]Failed to start teaching path record: {}".format(str(error))
            log.LogError(self.__node_name,err_msg)
            rospy.logerr(err_msg)
        finally:
            return ret_dat

    def CancelTeachingPathRecord(self):
        '''
        取消示教路径记录
        '''
        #ret_dat = {};ret_dat["ret"]='success';ret_dat["code"]=0
        ret_dat =self.ResDataInit()   
        try:
            # teaching_path_req = GenerateTeachPathRequest()
            # teaching_path_req.flag = teaching_path_req.CANCEL_RECOED
            # res_data = self.__teaching_path_client(teaching_path_req) 
            # if  res_data.success: raise Exception("service</teach_path_generate> error: {}".format(res_data.message))
            path_gen_msg = PathGenerate()
            path_gen_msg.path_types.append(
                self.GenPathGenerateType(PathGenerateType.MANUAL_METHOD)
            )
            path_gen_msg.path_action.data = PathGenerateAction.CANCEL_RECORD
            client_data = TaskGenerateManagerRequest()
            client_data.option = TaskGenerateManagerRequest.CREATE_TASK
            client_data.task_base.path_gen = path_gen_msg
            client_data.task_base.task_type.type = TaskType.TEACH_TASK
            client_data.task_base.control_mode.mode = TaskControlMode.SWEEP_EDGE_MODE
            resp = self.__task_generate_client.call(client_data)
            if not resp.response.success:
                raise Exception(resp.response.message)           
            
            log_msg = "[TaskParser]Successful to cancel teaching path record."
            log.LogInfo(self.__node_name,log_msg)
        except Exception as error:
            #ret_dat["ret"] = 'fail'
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[TaskParser]Failed to cancel teaching path record: {}".format(str(error))
            log.LogError(self.__node_name,err_msg)
            rospy.logerr(err_msg)
        finally:
            return ret_dat

    def SaveTeachingPathRecord(self,data):
        '''
        保存示教路径记录
        '''
        #ret_dat = {};ret_dat["ret"]='success';ret_dat["code"]=0
        ret_dat = self.ResDataInit()   
        try:
            if not data: raise Exception("Request data error!")
            task_name = data["path_name"].encode('utf-8')
            description = data["desc"].encode('utf-8')

            path_gen_msg = PathGenerate()
            path_gen_msg.path_types.append(
                self.GenPathGenerateType(PathGenerateType.MANUAL_METHOD)
            )
            path_gen_msg.path_action.data = PathGenerateAction.FINISHED_RECORD
 
            task_base = TaskBase()
            task_base.task_name = task_name
            task_base.path_gen = path_gen_msg
            task_base.task_type.type = TaskType.TEACH_TASK
            task_base.control_mode.mode = TaskControlMode.SWEEP_EDGE_MODE
            #指定任务执行速度
            clean_speed = int(data["clean_speed"])
            if clean_speed == 1:
                task_base.task_speed.speed = task_base.task_speed.LOW_SPEED
            if clean_speed == 2:
                task_base.task_speed.speed = task_base.task_speed.MIDDLE_SPEED
            if clean_speed == 3:
                task_base.task_speed.speed = task_base.task_speed.HIGH_SPEED
            # 外设控制模式
            if "utils_ctrl_default_item_name" in data:
                task_base.utils_mode = self.CheckUtilsName(data["utils_ctrl_default_item_name"].encode('utf-8'))

            client_data = TaskGenerateManagerRequest()
            client_data.option = TaskGenerateManagerRequest.CREATE_TASK
            client_data.task_base = task_base

            resp = self.__task_generate_client.call(client_data)
            if not resp.response.success:
                raise Exception(resp.response.message)   

            log_msg = "[TaskParser]Craete a new teach task<{}>".format(task_name)
            log.LogInfo(self.__node_name,log_msg)

        except Exception as error:
            #ret_dat["ret"] = 'fail'
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[TaskParser]Failed to save teaching path: {}".format(str(error))
            log.LogError(self.__node_name,err_msg)
            rospy.logerr(err_msg)
        finally:
            return ret_dat

    def GetIP(self):
        """
            获取IP
        """
        IP = get_extract_ip.extract_ip()
        return IP

    def ReportTaskList(self,data):
        '''
            报告清扫任务列表 
        '''    
        #ret_dat = {};ret_dat["ret"]='success';ret_dat["code"]=0
        ret_dat = {};ret_dat["msg"]='success';ret_dat["code"]=0     
        try:
            if not data: raise Exception("Request data error!")

            start_date = data["startDate"]
            if not start_date: raise Exception("start_date is empty!")

            end_date = data["endDate"]
            if not end_date: raise Exception("end_date is empty!")

            page = None                             # 增加分页功能 20231010-dw.liu
            page_size = None
            if "page" in data:
                '''当前第几页从1开始'''
                page =  data["page"]
            if "pageSize" in data:
                '''一页显示多少条'''
                page_size =  data["pageSize"]
            # print("========page========")
            # print(page)
            # print(pageSize)


            ##等待已执行任务记录功能实现后，添加
            #返回默认
            #__task_record_query_client
            ##查询数据库中任务数据
            task_record_req = TaskReportManagerRequest()
            task_record_req.option = task_record_req.QUERY_TASK_RECORD_BYTIME
            if page:
                task_record_req.page = page
                task_record_req.page_size = page_size
            
            task_record_req.start_time.secs =int(time.mktime(time.strptime(start_date,"%Y-%m-%d %H:%M:%S")))
            task_record_req.end_time.secs =int(time.mktime(time.strptime(end_date,"%Y-%m-%d %H:%M:%S")))

            res_data = self.__task_record_client(task_record_req)
            # v3_msgs/TaskGroup task_group            #任务组数据
            # string start_time                       #开始执行时间
            # string end_time                         #结束执行时间
            # float32 working_time                    #实际执行耗时 单位秒
            # float32 total_area                      #总清洁面积
            # float32 cleaned_area                    #已清洁面积
            # float32 uncleaned_area                  #未清扫面积：工作总面积/工作面积
            # float32 cleaning_efficiency             #清洁效率：工作面积/工作时长 
            # float32 coverage_rate                   #覆盖率：工作面积/工作总面积
            # v3_msgs/ErrorCode error_code            #错误码
            # v3_msgs/TaskPicture[] task_pictures     #任务图片
            task_recod_list=[]
            prefix = "home/aoting/user_config/"
            for record in res_data.task_records:
                record_info = {}
                record_info["task_name"]=record.task_group.group_name
                record_info["map_name"]=""
                record_info["start_time_sort"]=record.start_time
                # 暂时读取第一个任务地图和状态
                if record.task_group.task_list:
                    record_info["map_name"]= record.task_group.task_list[0].task_base.map_base.name
                record_info["add_time"]=""
                record_info["start_time"]=record.start_time
                record_info["end_time"]=record.end_time
                record_info["actual_working_duration"]=record.working_time
                record_info["theoretic_cov_rate"]=record.total_area
                record_info["working_area"]=record.cleaned_area
                record_info["no_working_area"]=record.uncleaned_area
                png_file_path = ""
                if record.task_pictures:
                    png_file_path = record.task_pictures[0].png_file_path
                    prefix_index = png_file_path.find(prefix)
                    if prefix_index != -1 :
                        png_file_path = png_file_path[prefix_index + len(prefix):]
                record_info["map_url"]= "http://" + self.GetIP() + ":5000/robot_files/{}".format(png_file_path)
                record_info["cov_rate"]=record.coverage_rate
                record_info["status_code"]= 7
                record_info["executed"]= 1.0 #
                record_info["is_cancel"]= 0
                # print(record.task_group.group_name, record.task_group.group_id, record.status)
                if record.status.status == TaskState.USER_CANCEL:
                    record_info["is_cancel"]= 1
                record_info["error_msg"] = record.error_code.error_msg
                task_recod_list.append(record_info)
                # print(record_info["is_cancel"])
            ret_dat["task_record_list"] = sorted(task_recod_list,key=lambda task_recod_one:task_recod_one['start_time_sort'],reverse=True)
            ret_dat["isSupportPagination"] = 1 # 支持分页
            log_msg = "[TaskParser]Successful to report task history record."
            log.LogInfo(self.__node_name,log_msg)
        except Exception as error:
            ret_dat["code"]=-1
            ret_dat["msg"]=str(error)

            err_msg = "[TaskParser]Failed to report task history record: {}".format(str(error))
            log.LogError(self.__node_name,err_msg)
            rospy.logerr(err_msg)
        finally:
            return ret_dat    

    def PushTaskStart(self):
        '''
            推行任务开始
        '''
        ret_dat =self.ResDataInit()   
        try:
            path_gen_msg = PathGenerate()
            path_gen_msg.path_types.append(
                self.GenPathGenerateType(PathGenerateType.MANUAL_POLYGON_METHOD)
            )
            path_gen_msg.path_action.data = PathGenerateAction.START_RECORD
            client_data = TaskGenerateManagerRequest()
            client_data.option = TaskGenerateManagerRequest.CREATE_TASK
            client_data.task_base.path_gen = path_gen_msg
            client_data.task_base.task_type.type = TaskType.TEACH_TASK
            resp = self.__task_generate_client.call(client_data)
            if not resp.response.success:
                raise Exception(resp.response.message)             
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)
            err_msg = "[PushTaskStart] error: {}".format(str(error))
            rospy.logerr(err_msg)
            log.LogError(self.__node_name,err_msg)
        finally:
            return ret_dat 


    def PushTaskCanel(self):
        '''
            推行任务取消
        '''
        ret_dat =self.ResDataInit()   
        try:
            path_gen_msg = PathGenerate()
            path_gen_msg.path_types.append(
                self.GenPathGenerateType(PathGenerateType.MANUAL_POLYGON_METHOD)
            )
            path_gen_msg.path_action.data = PathGenerateAction.CANCEL_RECORD
            client_data = TaskGenerateManagerRequest()
            client_data.option = TaskGenerateManagerRequest.CREATE_TASK
            client_data.task_base.path_gen = path_gen_msg
            client_data.task_base.task_type.type = TaskType.TEACH_TASK
            resp = self.__task_generate_client.call(client_data)
            if not resp.response.success:
                raise Exception(resp.response.message)             
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)
            err_msg = "[PushTaskCanel] error: {}".format(str(error))
            rospy.logerr(err_msg)
            log.LogError(self.__node_name,err_msg)
        finally:
            return ret_dat 

    def GetCirclePointList(self,radir,center):
        '''
        已知圆心坐标和圆半径，求圆边点坐标 
        radir 半径,center圆心
        '''
        r = radir
        c_x = center["x"]
        c_y = center["y"]
        average = 10 # 圆平均分为10份
        rad = (2*math.pi)/average # 一个圆分成10份，每一份弧度为 rad
        point_list = []
        for i in range(0,average+1):
            point_one = {}
            x = c_x + r*math.sin(rad*i)
            y = c_y + r*math.cos(rad*i)
            point_one["x"] = x
            point_one["y"] = y
            point_list.append(point_one)
 
        return  point_list

    def PushTaskSave(self, data):
        '''
            推行任务保存
        '''
        ret_dat =self.ResDataInit()   
        try:
            if not data : raise Exception("Request data error!")
            if "map_name" not in data: 
                raise Exception("map_name is empty!")
            if "task_name" not in data: 
                raise Exception("task_name is empty!")
            map_name = data["map_name"].encode('utf-8')
            task_name = data["task_name"].encode('utf-8')

            task_base = TaskBase()
            task_base.map_base.name = map_name
            task_base.task_name = task_name
            task_base.task_type.type = TaskType.TEACH_TASK

            #路径是否静态，1：静态路径 0：动态路径每次执行计算路径
            task_base.dynamic_adjust_path = data["task_is_static"] == 0

            # 外设控制模式
            if data.has_key("utils_ctrl_default_item_name"):
                task_base.utils_mode = self.CheckUtilsName(data["utils_ctrl_default_item_name"].encode('utf-8'))


            # 起点
            if "path_start" not in data : 
                raise Exception("path_start is empty!")
            task_base.start.position.x = data["path_start"]["x"]
            task_base.start.position.y = data["path_start"]["y"]

            #指定任务执行速度
            clean_speed = int(data["clean_speed"])
            if clean_speed == 1:
                task_base.task_speed.speed = task_base.task_speed.LOW_SPEED
            if clean_speed == 2:
                task_base.task_speed.speed = task_base.task_speed.MIDDLE_SPEED
            if clean_speed == 3:
                task_base.task_speed.speed = task_base.task_speed.HIGH_SPEED

            # if task_one.has_key("note") : 
            #     note = task_one["note"].encode('utf-8')

            path_gen_msg = PathGenerate()
            path_gen_msg.path_types.append(
                self.GenPathGenerateType(PathGenerateType.MANUAL_POLYGON_METHOD)
            )
            path_gen_msg.path_action.data = PathGenerateAction.FINISHED_RECORD
            # 危险区域
            if "origin_exclude" in data:
                origin_exclude_list = data["origin_exclude"]
                for origin_exclude_one in origin_exclude_list:
                    if len(origin_exclude_one) < 3 : raise Exception("Invaild cleaning area!")
                    danger_area_polygon_one = PolygonData()
                    for origin_point in origin_exclude_one:
                        pose_data = Pose()
                        pose_data.position.x = origin_point["x"]
                        pose_data.position.y = origin_point["y"]
                        danger_area_polygon_one.poses.append(pose_data)
                    path_gen_msg.danger_areas.append(danger_area_polygon_one)
            if "origin_exclude_circle" in data:
                origin_exclude_circle_list = data["origin_exclude_circle"]
                for origin_exclude_circle_one in origin_exclude_circle_list:
                    circle_point_list = self.GetCirclePointList(origin_exclude_circle_one["radir"],origin_exclude_circle_one["center"])
                    danger_area_polygon_one = PolygonData()
                    for origin_point in circle_point_list:
                        pose_data = Pose()
                        pose_data.position.x = origin_point["x"]
                        pose_data.position.y = origin_point["y"]
                        danger_area_polygon_one.poses.append(pose_data)
                    path_gen_msg.danger_areas.append(danger_area_polygon_one)

            clean_mode = data["mode"]
            print("clean_mode: {}".format(clean_mode))
            task_base.control_mode, path_gen_msg.path_types_child = self.CleanModeParser(clean_mode)
            task_base.path_gen = path_gen_msg
            
            client_data = TaskGenerateManagerRequest()
            client_data.option = TaskGenerateManagerRequest.CREATE_TASK
            client_data.task_base = task_base
            resp = self.__task_generate_client.call(client_data)
            if not resp.response.success:
                raise Exception(resp.response.message)      

        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)
            err_msg = "[PushTaskSave] error: {}".format(str(error))
            rospy.logerr(err_msg)
            log.LogError(self.__node_name,err_msg)
        finally:
            return ret_dat 

    def PushTaskPre(self, data):
        '''
            推行任务路径预览
        '''
        ret_dat =self.ResDataInit()   
        try:
            if not data : raise Exception("Request data error!")
            print("[PushTaskPre] data: ", data)
            if "map_name" not in data: 
                raise Exception("map_name is empty!")
            map_name = data["map_name"].encode('utf-8')

            task_base = TaskBase()
            task_base.map_base.name = map_name
            task_base.task_type.type = TaskType.TEACH_TASK

            # #路径是否静态，1：静态路径 0：动态路径每次执行计算路径
            # task_base.dynamic_adjust_path = data["task_is_static"] == 0

            # 外设控制模式
            if data.has_key("utils_ctrl_default_item_name"):
                task_base.utils_mode = data["utils_ctrl_default_item_name"].encode('utf-8')

            # 起点
            if "path_start" not in data : 
                raise Exception("path_start is empty!")
            task_base.start.position.x = data["path_start"]["x"]
            task_base.start.position.y = data["path_start"]["y"]

            # #指定任务执行速度
            # clean_speed = int(data["clean_speed"])
            # if clean_speed == 1:
            #     task_base.task_speed.speed = task_base.task_speed.LOW_SPEED
            # if clean_speed == 2:
            #     task_base.task_speed.speed = task_base.task_speed.MIDDLE_SPEED
            # if clean_speed == 3:
            #     task_base.task_speed.speed = task_base.task_speed.HIGH_SPEED

            # if task_one.has_key("note") : 
            #     note = task_one["note"].encode('utf-8')

            path_gen_msg = PathGenerate()
            path_gen_msg.path_types.append(
                self.GenPathGenerateType(PathGenerateType.MANUAL_POLYGON_METHOD)
            )
            path_gen_msg.path_action.data = PathGenerateAction.FINISHED_RECORD
            # 危险区域
            if "origin_exclude" in data:
                origin_exclude_list = data["origin_exclude"]
                for origin_exclude_one in origin_exclude_list:
                    if len(origin_exclude_one) < 3 : raise Exception("Invaild cleaning area!")
                    danger_area_polygon_one = PolygonData()
                    for origin_point in origin_exclude_one:
                        pose_data = Pose()
                        pose_data.position.x = origin_point["x"]
                        pose_data.position.y = origin_point["y"]
                        danger_area_polygon_one.poses.append(pose_data)
                    path_gen_msg.danger_areas.append(danger_area_polygon_one)
            if "origin_exclude_circle" in data:
                origin_exclude_circle_list = data["origin_exclude_circle"]
                for origin_exclude_circle_one in origin_exclude_circle_list:
                    circle_point_list = self.GetCirclePointList(origin_exclude_circle_one["radir"],origin_exclude_circle_one["center"])
                    danger_area_polygon_one = PolygonData()
                    for origin_point in circle_point_list:
                        pose_data = Pose()
                        pose_data.position.x = origin_point["x"]
                        pose_data.position.y = origin_point["y"]
                        danger_area_polygon_one.poses.append(pose_data)
                    path_gen_msg.danger_areas.append(danger_area_polygon_one)

            clean_mode = data["mode"]
            print("clean_mode: {}".format(clean_mode))
            task_base.control_mode, path_gen_msg.path_types_child = self.CleanModeParser(clean_mode)
            task_base.path_gen = path_gen_msg
            
            client_data = TaskGenerateManagerRequest()
            client_data.option = TaskGenerateManagerRequest.PREVIEW_TASK
            client_data.task_base = task_base
            resp = self.__task_generate_client.call(client_data)
            if not resp.response.success:
                raise Exception(resp.response.message) 

            # 返回预览路径
            ret_dat["origin"] = list()  # 轮廓数据
            res_task_base = resp.task_single.task_base
            for pose in res_task_base.polygon.poses:
                point_one = {
                    "x" : pose.position.x,
                    "y" : pose.position.y
                }
                ret_dat["origin"].append(point_one)

            ret_dat["sub_tasks"] = list()   # 路径信息
            # 常规路径
            for task_path in res_task_base.paths:
                point_dict = {
                    "path" : []
                }
                for pose in task_path.poses:
                    point_one = {
                        "x" : pose.position.x,
                        "y" : pose.position.y
                    }
                    point_dict["path"].append(point_one)
                ret_dat["sub_tasks"].append(point_dict)
            # 贴边路径
            for task_path in res_task_base.edge_paths:
                point_dict = {
                    "path" : []
                }
                for pose in task_path.poses:
                    point_one = {
                        "x" : pose.position.x,
                        "y" : pose.position.y
                    }
                    point_dict["path"].append(point_one)
                ret_dat["sub_tasks"].append(point_dict)  
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)
            err_msg = "[PushTaskPre] error: {}".format(str(error))
            rospy.logerr(err_msg)
            log.LogError(self.__node_name,err_msg)
        finally:
            return ret_dat 
        
    def TaskRegenerate(self,data):
        ret_dat = self.ResDataInit()
        try:
            if not data : raise Exception("Request data error!")
            print(data)
            if "map_name" not in data: 
                raise Exception("map_name is empty!")
            map_name = data["map_name"].encode('utf-8')

            task_single = TaskSingle()
            task_single.task_base.map_base.name = map_name
            # 单个任务删除请求
            req = TaskSingleManagerRequest()
            req.option = req.UPDATE_TASK_PATH_WITH_MAP
            req.task_single = task_single
            resp = self.__task_single_client.call(req)
            if not resp.response.success:
                raise Exception(resp.response.message)
        except Exception as error:
            rospy.logerr("[TaskRegenerate] error: ".format(error))
            ret_dat = self.ResDataError(str(error))
        finally:
            return ret_dat     